#include "stdafx.h"
#include "ArqueroEjemplo.h"

ArqueroEjemplo::ArqueroEjemplo()
{
	number = 0;
}

void ArqueroEjemplo::OnPlay()
{
	double yball = env->predictedBall.pos.y;
	double r = NormalizarAngulo(robot->rotation);

	if (fabs(fabs(r)-90) < 5)
	{
		// Estoy apuntando mas o menos hacia arriba o abajo => avanzo o retrocedo
		if (fabs(robot->pos.y - yball) < 1)
		{
			robot->velocityLeft = 0;
			robot->velocityRight = 0;
		}
		else
		{
			if (robot->pos.y < yball)
			{
				if (robot->pos.y > GTOPY)
				{
					robot->velocityLeft = 0;
					robot->velocityRight = 0;
				}
				else
				{
					if (r > 0)		// Mira hacia arriba
					{
						robot->velocityLeft = MaxVel;
						robot->velocityRight = MaxVel;
					}
					else
					{
						robot->velocityLeft = -MaxVel;
						robot->velocityRight = -MaxVel;
					}
				}
			}
			else
			{
				if (robot->pos.y < GBOTY)
				{
					robot->velocityLeft = 0;
					robot->velocityRight = 0;
				}
				else
				{
					if (r > 0)		// Mira hacia arriba
					{
						robot->velocityLeft = -MaxVel;
						robot->velocityRight = -MaxVel;
					}
					else
					{
						robot->velocityLeft = MaxVel;
						robot->velocityRight = MaxVel;
					}
				}
			}
		}
	}
	else
	{
		// Ajusto orientacion
		if (r > 0)   // Miro para arriba
		{
			robot->velocityLeft = -(90-r) * ATENUAR;
			robot->velocityRight = (90-r) * ATENUAR;
		}
		else
		{
			robot->velocityLeft = (r+90) * ATENUAR;
			robot->velocityRight = -(r+90) * ATENUAR;
		}
	}
}